#include <pic18f4431.h>

#define VERSION "0203F0"
 
#define MAXRPMRA 240
#define MAXRPMDE 180

#define PRECISION 10

/* MicroController Frequency */
#define FOSC 40000000
#define TOSC 25

/* Leds */
#define LED_YELLOW PORTCbits.RC2
#define LED_GREEN PORTCbits.RC1
#define SETLEDOUTPUTPORTS TRISCbits.TRISC1=TRISCbits.TRISC2=0

/* Gears/Worms/Steppers */
#define RA_NB_TEETH 144
#define DE_NB_TEETH 144
#define RA_GEAR_RATIO 4
#define DE_GEAR_RATIO 6
#define RA_NB_STEPS 400
#define DE_NB_STEPS 400
//#define MICROSTEPS 8
//#define LNMICROSTEPS 3
#define MICROSTEPS 16
#define LNMICROSTEPS 4
#define RA_STEPS_360 ((unsigned long)RA_NB_TEETH * RA_GEAR_RATIO * RA_NB_STEPS * MICROSTEPS) 
#define DE_STEPS_360 ((unsigned long)DE_NB_TEETH * DE_GEAR_RATIO * DE_NB_STEPS * MICROSTEPS)
/* MICROSTEPS=8
#define RA_STEPS_360  0x1C2000
#define DE_STEPS_360  0x2A3000
*/
#define RA_STEPS_WORM ((unsigned long)RA_GEAR_RATIO * RA_NB_STEPS * MICROSTEPS)
#define DE_STEPS_WORM ((unsigned long)DE_GEAR_RATIO * DE_NB_STEPS * MICROSTEPS)
/* MICROSTEPS=8
#define RA_STEPS_WORM  0x3200
#define DE_STEPS_WORM  0x4B00
*/

/** Motors **/
#define PWMPERIOD 0x007F
#define RAPHIA PORTDbits.RD5
#define RAPHIB PORTDbits.RD6
#define RAENABLE PORTBbits.RB0
// ADPWMA = PWM1
#define RAPWMA PORTBbits.RB1 
// ADPWMB = PWM5
#define RAPWMB PORTBbits.RB4
#define SETRAOUTPUTPORTS TRISDbits.TRISD5=TRISDbits.TRISD6=TRISBbits.TRISB0=0

#define SETRAPWMA(duty) PDC0L = duty & 0xFF; PDC0H = (duty >> 8) & 0x3F
#define SETRAPWMB(duty) PDC2L = duty & 0xFF; PDC2H = (duty >> 8) & 0x3F


#define DEPHIA PORTCbits.RC4
#define DEPHIB PORTCbits.RC5
#define DEENABLE PORTDbits.RD4
// DEPWMA = PWM3
#define DEPWMA PORTBbits.RB3 
// DEPWMB = PWM7
#define DEPWMB PORTDbits.RD7
#define SETDEOUTPUTPORTS TRISCbits.TRISC4=TRISCbits.TRISC5=TRISDbits.TRISD4=0
#define SETDEPWMA(duty) PDC1L = duty & 0xFF; PDC1H = (duty >> 8) & 0x3F
#define SETDEPWMB(duty) PDC3L = duty & 0xFF; PDC3H = (duty >> 8) & 0x3F

#define SETPWMA(axisindex, duty) if (axisindex==0) {SETRAPWMA(duty);} else {SETDEPWMA(duty);}
#define SETPWMB(axisindex, duty) if (axisindex==0) {SETRAPWMB(duty);} else {SETDEPWMB(duty);}

/* Microstepping */
/* 8 microsteps */
/* #define MICROSTEP_MASK 0x07 */
#define MICROSTEP_MASK (MICROSTEPS - 1)
// PWM_MASK = 0x1 << LN(MICROSTEPS)
// #define PWM_MASK 0x08 
#define PWM_MASK (0x1 << LNMICROSTEPS)
// WINDINGB_MASK = 0x2 << LN(MICROSTEPS)
//#define WINDINGB_MASK 0x10
#define WINDINGB_MASK (0x2 << LNMICROSTEPS)
// WINDINGA_MASK = 0x3 << LN(MICROSTEPS)
//#define WINDINGA_MASK 0x18
#define WINDINGA_MASK (0x3 << LNMICROSTEPS)

#define SETRAPHIA(microstep) if ((((microstep & WINDINGA_MASK) == 0) || ((microstep & WINDINGA_MASK) == WINDINGA_MASK))!=0) RAPHIA=1; else RAPHIA=0
#define SETRAPHIB(microstep) if ((microstep & WINDINGB_MASK) !=0) RAPHIB=1; else  RAPHIB=0

#define SETDEPHIA(microstep) if ((((microstep & WINDINGA_MASK) == 0) || ((microstep & WINDINGA_MASK) == WINDINGA_MASK))!=0) DEPHIA=1; else DEPHIA=0
#define SETDEPHIB(microstep) if ((microstep & WINDINGB_MASK) !=0) DEPHIB=1; else  DEPHIB=0

#define SETPHIA(axisindex) if (axisindex==0) {SETRAPHIA(motors[0].microstep);} else {SETDEPHIA(motors[1].microstep);}
#define SETPHIB(axisindex) if (axisindex==0) {SETRAPHIB(motors[0].microstep);} else {SETDEPHIB(motors[1].microstep);}

/* Motor status (Skywatcher protocol) */
#define INITIALIZED 0x0100
#define RUNNING 0x0001
#define SLEWMODE 0X0010
#define BACKWARD 0x0020
#define HIGHSPEED 0x0040
#define SETMOTORPROPERTY(motorstatus, property) motorstatus |= property
#define UNSETMOTORPROPERTY(motorstatus, property) motorstatus &= ~property
#define GETMOTORPROPERTY(motorstatus, property) (motorstatus & property)

/* Timer period */
#define MICROSECONDS 1000000
#define MUL_RA (MICROSECONDS / RA_STEPS_WORM)
#define REM_RA (MICROSECONDS % RA_STEPS_WORM)
#define MUL_DE (MICROSECONDS / DE_STEPS_WORM)
#define REM_DE (MICROSECONDS % DE_STEPS_WORM)
#define TICKS_MICRO 1000 / (4 * TOSC)
#define MIN_TICKS (PWMPERIOD + 1) * 16


typedef union {
  unsigned int value;
  struct { unsigned char valuel; unsigned char valueh; } fields;
} TimerRegister;

typedef struct {
  unsigned char count;
  unsigned char current;
  TimerRegister timer;
} TimerValue;

#define SETTIMER(axisindex) if (axisindex==0) { TMR1H=motors[0].timer.timer.fields.valueh;TMR1L=motors[0].timer.timer.fields.valuel; } \
  else { TMR0H=motors[1].timer.timer.fields.valueh;TMR0L=motors[1].timer.timer.fields.valuel; }

#define SETREG(reg, mask) *reg |= mask
#define UNSETREG(reg, mask) *reg &= ~mask

#define STARTTIMER(axisindex) if (axisindex==0) T1CONbits.TMR1ON = 1; else T0CONbits.TMR0ON = 1;
#define STOPTIMER(axisindex) if (axisindex==0) T1CONbits.TMR1ON = 0; else T0CONbits.TMR0ON = 0;

/* Motor acceleration/deceleration */
// Speed ramp states
#define STOP  0
#define ACCEL 1
#define DECEL 2
#define RUN   3

typedef struct {
  //! What part of the speed ramp we are in.
  unsigned char run_state : 3;
  //! Peroid of next timer delay. At start this value set the accelration rate.
  unsigned long step_delay;
  unsigned int rest;
  //! What step_pos to start decelaration
  unsigned long decel_start;
  //! Sets deceleration rate.
  unsigned int decel_val;
  //! Minimum time delay (max speed)
  unsigned long min_delay;
  //! Counter used when accelerateing/decelerateing to calculate step_delay.
  unsigned int accel_count;
} speedRampData;


typedef struct {
  unsigned long mul_period;
  unsigned long rem_period;
  unsigned long steps_worm;
  unsigned long steps_360;
  unsigned int status; // lowspeed, forward, slew mode, stopped
  unsigned long position;
  unsigned char microstep;
  unsigned char pwmindex;
  unsigned long wormperiod;
  unsigned long minperiod;
  unsigned long target;
  unsigned long target_current;
  unsigned long target_slow;
  unsigned long breaks;
  TimerValue timer;
  speedRampData speedramp;
  unsigned char id;
} AxisData;


AxisData motors[] = {
  {
    .status=0X0010,
    .position = 0x800000,
    .microstep = 0x00,
    .pwmindex = MICROSTEPS - 1,
    .wormperiod = 0x256,
    .minperiod =   (60 * RA_GEAR_RATIO * PRECISION) / MAXRPMRA , 
    .target = 0x000001, 
    .target_current = 0x000000,
    .target_slow = 400,
    .breaks = 400,
    .timer= {
      .count=0,
      .current=0,
      .timer={ .value=0 }
    },
    .speedramp={
      .run_state = 3,
      .step_delay = 0,
      .rest = 0,
      .decel_start = 0,
      .decel_val = 0,
      .min_delay = 0,
      .accel_count = 0
    },
    .id = '1'
  },
  {
    .status=0X0010,
    .position = 0x800000,
    .microstep = 0x00,
    .pwmindex = MICROSTEPS - 1,
    .wormperiod = 0x256,
    .minperiod =    (60 * DE_GEAR_RATIO * PRECISION) / MAXRPMDE ,
    .target = 0x000001, 
    .target_current = 0x000000,
    .target_slow = 400,
    .breaks = 400,
    .timer= {
      .count=0,
      .current=0,
      .timer={ .value=0 }
    },
    .speedramp={
      .run_state = 3,
      .step_delay = 0,
      .rest = 0,
      .decel_start = 0,
      .decel_val = 0,
      .min_delay = 0,
      .accel_count = 0
    },
    .id = '2'
  }
};


/* Acceleration parameters (from http://atmel.com/dyn/resources/prod_documents/doc8017.pdf) */
/* http://eetimes.com/design/embedded/4006438/Generate-stepper-motor-speed-profiles-in-real-time */
#define TIMER_FREQ (MICROSECONDS * TICKS_MICRO)
#define ALPHA (2*M_PI) / RA_NBSTEPS 
#define AT_100 (ALPHA * TIMER_FREQ * 100)
#define TFREQ (TIMER_FREQ * 0.676) / 100
#define A_SQ (2 * ALPHA * 10000000000)
#define A_x20000 (2 * ALPHA * 10000)
/* c0 = f * sqrt(2 * alpha / accel) */
/* for accel = (M_PI * 2) rad/s2 */
/*#define C0 707106
#define C0_APPROX 478004
#define C0_TIMER_COUNT 8
#define C0_TIMER_VALUEH 0x16
#define C0_TIMER_VALUEL 0x9A
*/
/* for accel = (M_PI * 4) rad/s2 */
#define C0 500000
#define C0_APPROX 338000
#define C0_TIMER_COUNT 8
#define C0_TIMER_VALUEH 0x5A
#define C0_TIMER_VALUEL 0xF6

